#!/usr/bin/env python
# encoding: utf-8

import sys
import time
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_srvs.srv import Empty

class CartesianControl(object):
    def __init__(self):
        # 初始化节点
        super(CartesianControl, self).__init__()
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('get_current_pose')

        try:
            # 检查是否存在夹持器
            self.is_gripper_present = rospy.get_param(rospy.get_namespace() + "is_gripper_present", False)
            if self.is_gripper_present:
                gripper_joint_names = rospy.get_param(rospy.get_namespace() + "gripper_joint_names", [])
                self.gripper_joint_name = gripper_joint_names[0]
            else:
                gripper_joint_name = ""
            # 获取自由度
            self.degrees_of_freedom = rospy.get_param(rospy.get_namespace() + "degrees_of_freedom", 7)

            # 创建 MoveItInterface 对象
            arm_group_name = "arm"
            self.robot = moveit_commander.RobotCommander("robot_description")
            self.arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace())
            rospy.loginfo("Initializing node in namespace " + rospy.get_namespace())
            
            # 订阅笛卡尔坐标系控制话题
            self.cartesian_subscriber = rospy.Subscriber("cartesian_control_topic", geometry_msgs.msg.Pose, self.cartesian_control_callback)

            self.is_init_success = True
        except Exception as e:
            print (e)
            self.is_init_success = False

    def cartesian_control_callback(self, pose_msg):
        # 处理笛卡尔坐标系控制命令的回调函数
        rospy.loginfo("Received Cartesian pose control command")
        self.reach_cartesian_pose(pose_msg)

    def reach_cartesian_pose(self, pose_msg):
        arm_group = self.arm_group

        # 设置目标笛卡尔姿态
        target_pose = geometry_msgs.msg.PoseStamped()
        target_pose.header.frame_id = "base_link"
        target_pose.pose = pose_msg

        # 规划并执行轨迹
        arm_group.set_pose_target(target_pose)
        arm_group.go(wait=True)

def main():
    cartesian_control = CartesianControl()
    current_pose = cartesian_control.arm_group.get_current_pose().pose

    rospy.loginfo("Current Cartesian pose:")
    rospy.loginfo("Position: x={}, y={}, z={}".format(current_pose.position.x, current_pose.position.y, current_pose.position.z))
    rospy.loginfo("Orientation: x={}, y={}, z={}, w={}".format(current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w))

    rospy.spin()
    

if __name__ == '__main__':
    main()